/*
 * @Author       : Zybz
 * @Date         : 2022-04-20 18:53:25
 * @LastEditors  : Zybz
 * @LastEditTime : 2022-05-03 21:49:57
 * @FilePath     : \dji_driver_v2.0\Core\oslib\utils\utils.c
 * @Description  :
 *
 * Copyright (c) 2022 by BUPT RobotTeam, All Rights Reserved.
 */
#include "utils.h"
#include "oslib.h"
void PID_init()
{
}

float PID_Release(PID_Struct *PID, float target, float now)
{
    float err;
    float err_dt;
    float result;

    err = target - now;
    err_dt = err - PID->last_err;

    err_dt *= 0.384f;
    err_dt += PID->last_d * 0.615f; //低通滤波

    PID->last_err = err;

    PID->i += err * PID->I_TIME;

    Limit(PID->i, PID->i_max);
    PID->last_d = err_dt;

    result = err * PID->KP + err_dt * PID->KD + PID->i * PID->KI;
    return result;
}

void reset_PID(PID_Struct *s)
{
    s->i = 0;
    s->last_err = 0;
    s->last_d = 0;
}

void get_tick(Time_s *time)
{
    time->last_tick = time->tick;
    time->tick = osKernelGetTickCount(); // 毫秒数
    time->delta_tick = time->tick - time->last_tick;
    time->s = time->tick / 1000.0f; // 秒
}

void reset_time(Time_s *time)
{
    time->tick = 0;
    time->last_tick = 0;
    time->s = 0;
    time->delta_tick = 0;
}